#!/usr/bin/env python
#!coding=utf-8
 
import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2

from get_object_place_from_view  import get_target_place
cam0_path  = 'src/data/image/'    # 已经建立好的存储cam0 文件的目录


import sys

import rospy
from moveit_commander import MoveGroupCommander, roscpp_initialize
import moveit_commander
from geometry_msgs.msg import PoseStamped
from moveit_commander import PlanningSceneInterface
from rospy.rostime import get_time
# remap again

class move_arm():
    def __init__(self):
        joint_state_topic = ['joint_states:=/j2n6s300/joint_states']
        moveit_commander.roscpp_initialize(sys.argv)
        self.arm = MoveGroupCommander('arm')
        self.arm.set_max_acceleration_scaling_factor(1)
        self.arm.set_max_velocity_scaling_factor(1)
        self.arm.set_pose_reference_frame("root")
        self.end_effector_link = self.arm.get_end_effector_link()
        self.arm.set_start_state_to_current_state()
        scene = PlanningSceneInterface()


        self.gripper = MoveGroupCommander('gripper')
        self.gripper.set_max_acceleration_scaling_factor(1)
        self.gripper.set_max_velocity_scaling_factor(1)
        

    def getpose(self,x,y,z):
        target_pose = PoseStamped()
        target_pose.header.frame_id = "root"
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = x
        target_pose.pose.position.y = y
        target_pose.pose.position.z = z
        target_pose.pose.orientation.x = 0
        target_pose.pose.orientation.y = 0
        target_pose.pose.orientation.z = 0
        target_pose.pose.orientation.w = 1
        return target_pose

    def close_gripper(self):
        for _ in range(5):
            self.gripper.set_joint_value_target([1.1,1.1,1.1])
            self.gripper.go()
        print("Gripper Closed")
    def open_gripper(self):
        self.gripper.set_joint_value_target([0.1,0.1,0.1])
        self.gripper.go()
        print("Gripper Openned")


    def move_arm(self,x,y,z=0.3):
        target_pose =self.getpose(x,y,z)
        self.arm.set_pose_target(target_pose, "j2n6s300_link_6")
        self.arm.go()
    def grasp_object(self,x,y,z):
        self.open_gripper()
        self.move_arm(x,y,z+0.1)
        self.move_arm(x,y,z)
        self.close_gripper()
        self.move_arm(x,y,z+0.2)
        self.move_arm(0.3,0.3,0.3)
        self.move_arm(0.3,0.3,0.25)
        self.open_gripper()

def callback(data):
    # define picture to_down' coefficient of ratio


    cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
    timestr = "%.6f" %  data.header.stamp.to_sec()
            #%.6f表示小数点后带有6位，可根据精确度需要修改；
    image_name = timestr+ ".jpg" #图像命名：时间戳.jpg
    print(timestr)

    place = get_place.main(cv_img)
    print(place)
    for i in range(len(place[0])):
        move_Arm.grasp_object(x=place[0][i],y=place[1][i],z=0.3)
    print(count)

 
def displayWebcam():
    rospy.init_node('kinova_display', anonymous=True)
 
    # make a video_object and init the video object
    global count,bridge
    count = 0
    bridge = CvBridge()
    rospy.Subscriber('/camera/rgb/image_raw', Image, callback)
    print("subscriber")
    rospy.spin()



if __name__ == '__main__':
    get_place = get_target_place()
    move_Arm = move_arm()
    displayWebcam()

